
#ifndef __PATH_SMOOTHER_H__
#define __PATH_SMOOTHER_H__

#include <cmath>
#include <vector>
#include <memory>
#include <opencv2/opencv.hpp>

#include "dynamic_voronoi.h"
#include "common/constants.h"

using namespace global_path_planner::common;

namespace global_path_planner {
namespace path_smooth {

class PathSmoother {
public:
    PathSmoother() {};

    PathSmoother(cv::Mat map_img, const std::vector<common::Vec2d>& cv_path);

    void smoothPath(int& perim);

    // 梯度下降法路径平滑
    std::vector<std::pair<double, double>>
    PathSmooth(cv::Mat &grid_mat, double &grid_size,
               const std::pair<double, double> &grid_origin,
               std::vector<std::pair<double, double>> &vector_path);

    std::vector<common::Vec2d> getOriginalPath() {return original_path_;}
    std::vector<common::Vec2d> setOriginalPath(std::vector<common::Vec2d>& cv_path);

    std::vector<common::Vec2d> getSmoothedPath() {return smoothed_path_;}

    void setParams(float alpha, float wCurvature,
                   float wSmoothness, int maxIter,
                   float wVoronoi = 0.0, float wObstacle = 0.0);

private:

    common::Vec2d obstacleTerm(common::Vec2d xi);//障碍物项，用于约束路径远离障碍物

    common::Vec2d curvatureTerm(common::Vec2d xi0, common::Vec2d xi1, common::Vec2d xi2);//曲率项，用于保证可转弯性及通行性

    //平滑项，用于将节点等距分布并尽量保持同一个方向
    common::Vec2d smoothnessTerm(common::Vec2d xim, common::Vec2d xi, common::Vec2d xip);

    common::Vec2d voronoiTerm(common::Vec2d xi);

    bool isOnGrid(common::Vec2d vec);

    /// maximum possible curvature of the non-holonomic vehicle
    float kappaMax_ = 1.f / (constants::min_turn_radius * 1.1);
    /// maximum distance to obstacles that is penalized
    float obsDMax_ = 2 * constants::minRoadWidth;
    /// maximum distance for obstacles to influence the voronoi field
    float vorObsDMax_ = 2 * constants::minRoadWidth;
    //obsDMax的作用更显著，应该 vorObsDMax >= obsDMax。首先要不碰撞障碍物，在不碰的前提下，调整离障碍物的距离

    /// falloff rate for the voronoi field
    float alpha_ = 0.2;
    float wObstacle_ = 0.0;
    float wVoronoi_ = 0.0;
    float wCurvature_ = 6.0;
    float wSmoothness_ = 4.0;
    int maxIter_ = 0;

    // std::shared_ptr<DynamicVoronoi> voronoi_Ptr;
    cv::Mat map_img_;
    int map_width_;
    int map_height_;
    std::vector<common::Vec2d> original_path_;
    std::vector<common::Vec2d> smoothed_path_;
};

}  // namespace path_smooth
}  // namespace global_path_planner

#endif // __PATH_SMOOTHER_H__